/*
 * Kalman.cpp
 *
 *  Created on: 30 Nov 2012
 *      Author: Daniel Sun
*/

#include "rodos.h"
#include "Kalman.h"
#include "stdlib.h"
#include "../Threads/global.h"
extern Systemstate state;

Kalman::Kalman() {

	this->dt = SAMPLETIME_S;

//Set process noise quantities. Getting an improvement on these values might be necessary.
//	Q_angle = 0.0011;
	state.Q_angle = 0.0011;
	state.magVar = 0.1200;

//Set a lot of values to 0 (off)
	Starter = 0;
	S = 0;
	K = 0;
	rate = 0;
	y = 0;
	angle = 0;
	cnt = 0;
	kalmanInput = 0;
	angle_old = 0;
	angle_rep = 0;
	P = 0;
	oldAngleMag = 0;

}

//Make function to give the filtered angle
float Kalman::getAngle() {

	float newAngleMag = state.angleMag;
	float newRate = state.velGyro;
//If the Kalman has just started, angle estimate is that provided by magnetometer
	if (Starter == 0) {
		angle = newAngleMag;
		oldAngleMag = newAngleMag;
		kalmanInput = 0;
		Starter = 1;
	}
	kalmanInput += newAngleMag;
	// Detect crossing
#ifndef CAL_MAG
		if(abs(oldAngleMag - newAngleMag) > 180) {
			xprintf("Crossing detected! ");
			if(newAngleMag < 180){
				kalmanInput += 360;
				cnt--;
				xprintf("shift down\n");
			} else if(newAngleMag > 180){
				kalmanInput -= 360;
				cnt++;
				xprintf("shift up\n");
			}
		}
#endif
//Start Kalman filter algorithm
	angle_old = angle;
	rate = newRate - state.gyroBias;
	angle -= dt * rate;

//	xprintf("%f, %f", newAngle, angle);

	P += state.Q_angle;

	S = P + state.magVar;

	K = P / S;

	y = kalmanInput - angle;

	angle += K * y;

	P -= K * P;

	oldAngleMag = newAngleMag;
	kalmanInput -= oldAngleMag;
//	return angle;
	return angle + cnt * 360;
}

